#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
#include <time.h>
#include <sched.h>

#include <unistd.h>
#include <hw/inout.h>
#include <sys/neutrino.h>
#include <sys/mman.h>
#include <stdint.h>
#include <semaphore.h>

#include "AD.h"
#include "DA.h"
#include "history.h"

History * zhistory;

float ki, kd, kp;
float setpoint;

pthread_t inputGatherT, transformValueT, outputValueT;

sem_t doInput;
sem_t inputValueReady;
sem_t transformValueReady;
sem_t outputValueReady;

float inputValue, transformOutput, outputValue;



void PIDCalculateCurrent(History * hist) {
	setOutput(hist, 0,
			getInput(hist, 0) * (ki + kd + kp) - getInput(hist, -1)
			* (kp + 2 * kd) + getInput(hist, -2) * kd + getOutput(hist, -1));
}

void * inputGather(void * param) {
	int privity_err = ThreadCtl(_NTO_TCTL_IO, NULL );
	if (privity_err == -1) {
		fprintf(stderr, "can't get root permissions\n");
		return NULL;
	}
	while(1) {
		sem_wait(&doInput);
		inputValue = readValue();
		sem_post(&inputValueReady);
	}
}

void * transformValue(void* param) {
	while(1) {
		sem_wait(&inputValueReady);
		setInput(zhistory, 0, setpoint-inputValue);
		PIDCalculateCurrent(zhistory);
		transformOutput = getOutput(zhistory,0);
		stepTime(zhistory);
		sem_post(&transformValueReady);
	}
}
void * outputValues(void* param) {
	int privity_err = ThreadCtl(_NTO_TCTL_IO, NULL );
	if (privity_err == -1) {
		fprintf(stderr, "can't get root permissions\n");
		return NULL;
	}
	while(1) {
		sem_wait(&transformValueReady);
		writeValue(transformOutput);
	}
}

void doZTransform(union sigval val) {
	sem_post(&doInput);
}

void startPeriodicTaskTimer(void(*func)(union sigval), union sigval * value) {
	struct sigevent *sig = (struct sigevent*) malloc(sizeof(struct sigevent));
	struct itimerspec *timer = (struct itimerspec*) malloc(
			sizeof(struct itimerspec));
	timer_t timer_id;

	SIGEV_THREAD_INIT(sig, func, value, NULL);
	SIGEV_MAKE_CRITICAL(sig);

	timer_create(CLOCK_REALTIME, sig, &timer_id);

	timer -> it_interval.tv_nsec = 100000000;
	timer -> it_interval.tv_sec = 0;
	timer -> it_value.tv_nsec = 100000000;
	timer -> it_value.tv_sec = 0;

	timer_settime(timer_id, 0, timer, NULL);
}


void consumeLine() {
	while( getchar()!='\n' );
}

void doSetLine() {
	printf("\n\ni# set Ki     d# set Kd      p# set Kp    s# set setpoint     r# print settings\n");
	char var = getchar();
	float f;
	switch(var) {
	case 'i':
		scanf("%f",&f);
		ki = f;
		printf("set Ki to %f\n",ki);
		break;
	case 'd':
		scanf("%f",&f);
		kd = f;
		printf("set Kd to %f\n",kd);
		break;
	case 'p':
		scanf("%f",&f);
		kp = f;
		printf("set Kp to %f\n",kd);
		break;
	 case 's':
		scanf("%f",&f);
		setpoint = f;
		printf("set setpoint to %f\n",setpoint);
		break;
	 case 'r':
		printf("i:%.4f  d:%.4f   p:%.4f   s:%.4f\n",ki,kd,kp,setpoint);
		break;
	}
	consumeLine();
}

int main(int argc, char *argv[]) {

	zhistory = makeHistory(3);
	int privity_err;
	float f;

	sem_init(&doInput,0,0);
	sem_init(&inputValueReady,0,0);
	sem_init(&transformValueReady,0,0);
	sem_init(&outputValueReady,0,0);

	pthread_create(&inputGatherT, NULL, inputGather, NULL);
	pthread_create(&transformValueT, NULL, transformValue, NULL);
	pthread_create(&outputValueT, NULL, outputValues, NULL);


	kp = kd = ki = 0.1;
	/* Give this thread root permissions to access the hardware */
	privity_err = ThreadCtl(_NTO_TCTL_IO, NULL );
	if (privity_err == -1) {
		fprintf(stderr, "can't get root permissions\n");
		return -1;
	}

	initADRegister();
	initDARegister();

	startPeriodicTaskTimer(doZTransform, NULL);


	setpoint = 5.0;
	while(1) {
		doSetLine();
	}


	return 0;
}
